﻿#include "spline.h"

#ifdef NDEBUG
#define assert(expr) ((void)0)
#else
#define assert(expr) ((expr) ? (void)0 : __debugbreak())
#endif

int CAGD::Spline::FindSpan(double u, int deg, const std::vector<double> &knots)
{
    // 防止越界
    assert(u >= knots[deg]);
    assert(u <= knots[knots.size() - deg - 1]);

    // 返回左闭右开区间 u \in [u_i, u_{i+1})
    int m = knots.size() - 1;
    int n = m - deg - 1;

    // 边界情况：如果 u 在区间最大值的边界上，就返回左边的区间
    // 注意这里边界是最大的值，而不是节点最大值
    if (u == knots[knots.size() - deg - 1])
        return n;

    // 二分法寻找区间
    int low = deg;
    int high = n + 1;
    int mid = (low + high) / 2;
    while (u < knots[mid] || u >= knots[mid + 1])
    {
        if (u < knots[mid])
            high = mid;
        else
            low = mid;
        mid = (low + high) / 2;
    }
    return mid;
}

int CAGD::Spline::FindMult(int k, double u, int deg, const std::vector<double> &knots)
{
    int n = 0;
    while (k >= deg && knots[k] == u)
        n++, k--;
    return n;
}

std::vector<double> CAGD::Spline::Basis(int i, double u, int deg, const std::vector<double> &knots)
{
    // 防止越界，确保索引在有效区间内
    assert(i >= deg);
    assert(i < int(knots.size() - deg - 1));
    assert(u >= knots[deg]);
    assert(u <= knots[knots.size() - deg - 1]);

    std::vector<double> N(deg + 1);     // 存放 u \in [u_i, u_{i+1}) 处不为零的基函数值
    std::vector<double> left(deg + 1);  // left[j] = u - u_{i+1-j}
    std::vector<double> right(deg + 1); // right[j] = u_{i+j} - u

    N[0] = 1;
    for (int j = 1; j < deg + 1; j++)
    {
        left[j] = u - knots[i + 1 - j];
        right[j] = knots[i + j] - u;

        double saved = 0;
        for (int r = 0; r < j; r++)
        {
            double tmp = N[r] / (right[r + 1] + left[j - r]);
            N[r] = saved + right[r + 1] * tmp;
            saved = left[j - r] * tmp;
        }
        N[j] = saved;
    }
    return N;
}

std::vector<std::vector<double>> CAGD::Spline::DerivBasis(int deriv, int i, double u, int deg,
                                                          const std::vector<double> &knots)
{
    // 排除无效参数
    assert(deriv <= deg);
    assert(i >= deg);
    assert(i < int(knots.size() - deg - 1));
    assert(u >= knots[deg]);
    assert(u <= knots[knots.size() - deg - 1]);

    // ders[k][j] 表示 N_{i-deg+j}^(k)
    std::vector<std::vector<double>> ders(deriv + 1, std::vector<double>(deg + 1));

    // 先计算出非零基函数及节点差分
    std::vector<std::vector<double>> ndu(deg + 1, std::vector<double>(deg + 1));
    std::vector<double> left(deg + 1);  // left[j] = u - u_{i+1-j}
    std::vector<double> right(deg + 1); // right[j] = u_{i+j} - u

    ndu[0][0] = 1;
    for (int j = 1; j < deg + 1; j++)
    {
        left[j] = u - knots[i + 1 - j];
        right[j] = knots[i + j] - u;

        double saved = 0;
        for (int r = 0; r < j; r++)
        {
            // 下三角部分存放节点差分
            // ndu[j][r] = u_{i + r + 1} - u_{i + r + 1 - j}
            ndu[j][r] = right[r + 1] + left[j - r];

            // 上三角部分（包括对角元）存放所有非零不大于 deg 次基函数（AllBasisFuncs）
            // 注意这里 ndu[r][j] = N_{r,j}，没有像 AllBasisFuncs 一样反过来，因为空间已经分配好
            double tmp = ndu[r][j - 1] / ndu[j][r];
            ndu[r][j] = saved + right[r + 1] * tmp;
            saved = left[j - r] * tmp;
        }
        ndu[j][j] = saved;
    }

    // 将计算结果中的基函数部分（上三角）记录下来
    for (int j = 0; j < deg + 1; j++)
        ders[0][j] = ndu[j][deg];

    // N_{i,deg}^(k) = deg * [ N_{i,deg-1}^(k-1) / (u_{i+deg} - u_i) - N_{i+1,deg-1}^(k-1) / (u_{i+deg+1}-u_{i+1}) ]
    std::vector<std::vector<double>> a(2, std::vector<double>(deg + 1));
    for (int r = 0; r < deg + 1; r++)
    {
        // 计算 N_{i,r}^(k)
        double s1 = 0;
        double s2 = 1;

        // a_{0,0} = 1
        a[0][0] = 1;
        for (int k = 1; k < deriv + 1; k++)
        {
            double der = 0;

            // a_{k,0} = a_{k-1,0} / (u_{i+deg-k+1} - u_i)
            // 从 N_{i-deg+r,deg}^(k) 开始计算，这里 "i" 是 i-deg+r
            // 所以取 ndu[deg - k + 1][r - k] = u_{i+r-k+1} - u_{i-deg+r+j}
            if (r >= k)
            {
                // a[s2] 存放 a_{k, }, a[s1] 存放 a_{k-1, }
                a[s2][0] = a[s1][0] / ndu[deg - k + 1][r - k];
                der = a[s2][0] * ndu[r - k][deg - k];
            }

            // 正常情况下 j1 = 1, j2 = k - 1，就是完整的计算求和
            // 特殊情况下要排除掉系数 a 为零和基函数 N 为零的情况
            int j1, j2;
            if (r - k >= -1)
                j1 = 1;
            else
                j1 = -r + k;

            if (r - 1 <= deg - k)
                j2 = k - 1;
            else
                j2 = deg - r;

            // a_{k,j} = (a_{k-1,j} - a_{k-1,j-1}) / (u_{i+deg+j-k+1} - u_{i+j})
            // 注意这里 "i" 是 i-deg+r
            // j = 1, ..., k-1
            for (int j = j1; j < j2 + 1; j++)
            {
                a[s2][j] = (a[s1][j] - a[s1][j - 1]) / ndu[deg - k + 1][r - k + j];
                der += a[s2][j] * ndu[r - k + j][deg - k];
            }

            // 计算对角 a_{k,k} = -a_{k-1,k-1} / (u_{i+deg+1} - u_{i+k})
            // 注意这里 "i" 是 i-deg+r
            if (r <= deg - k)
            {
                a[s2][k] = -a[s1][k - 1] / ndu[deg - k + 1][r];
                der += a[s2][k] * ndu[r][deg - k];
            }
            ders[k][r] = der;

            // 交换行，将之前计算的 a_{k, } 作为下一个循环的 a_{k-1, }
            int s = s1;
            s1 = s2;
            s2 = s;
        }
    }

    // 由于重复求导，乘上因子 deg * (deg - 1) * ... * (deg - k + 1)
    int r = deg;
    for (int k = 1; k < deriv + 1; k++)
    {
        for (int j = 0; j < deg + 1; j++)
            ders[k][j] *= r;
        r *= deg - k;
    }
    return ders;
}

std::vector<std::vector<double>> CAGD::Spline::DerivAllBasis(int i, double u, int deg, const std::vector<double> &knots)
{
    assert(i >= deg);
    assert(i < int(knots.size() - deg - 1));
    assert(u >= knots[deg]);
    assert(u <= knots[knots.size() - deg - 1]);

    std::vector<std::vector<double>> N(deg + 1); // 存放 u \in [u_i, u_{i+1}) 处不为零的基函数值
    std::vector<double> left(deg + 1);           // left[j] = u - u_{i+1-j}
    std::vector<double> right(deg + 1);          // right[j] = u_{i+j} - u

    N[0].push_back(1);
    for (int j = 1; j < deg + 1; j++)
    {
        left[j] = u - knots[i + 1 - j];
        right[j] = knots[i + j] - u;

        // N[j][r] = N_{r,j}，即 j 次 r 下标
        // 这里之所以反过来，是为了方便 push_back
        double saved = 0;
        for (int r = 0; r < j; r++)
        {
            double tmp = N[j - 1][r] / (right[r + 1] + left[j - r]);
            N[j].push_back(saved + right[r + 1] * tmp);
            saved = left[j - r] * tmp;
        }
        N[j].push_back(saved);
    }
    return N;
}